from periphery import PWM
import time

# 初始化 PWM
pwm = PWM(2, 0)  # 替换为实际的 PWM 芯片和通道编号

try:
    pwm.polarity = "normal"

    # 设置 PWM 频率和占空比
    pwm.frequency = 50  # 50Hz
    pwm.duty_cycle = 0.075  # 初始占空比

    # 启动 PWM
    pwm.enable()

except:
    #pwm.polarity = "normal"

    # 设置 PWM 频率和占空比
    pwm.frequency = 50  # 50Hz
    pwm.duty_cycle = 0.075  # 初始占空比

    # 启动 PWM
    pwm.enable()

finally:
    pwm.polarity = "normal"

    # 设置 PWM 频率和占空比
    pwm.frequency = 50  # 50Hz
    pwm.duty_cycle = 0.075  # 初始占空比

    # 启动 PWM
    pwm.enable()

def set_angle(angle):
    # 将角度转换为占空比
    duty_cycle = (0.025 + (angle / 180.0) * 0.1)
    pwm.duty_cycle = duty_cycle
    time.sleep(1)

set_angle(0)    # 设置舵机到 0 度
time.sleep(2)
set_angle(90)   # 设置舵机到 90 度
time.sleep(2)
set_angle(180)  # 设置舵机到 180 度
time.sleep(2)

try:
    pwm.disable()
    pwm.close()
except Exception as e:
    print(e)

